﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_FusionTemp2Local.cs
 * \brief	This file contains all the neccesary functions to fusion and send the
 *          information stored in the aux_MapCell_local structure to the 
 *          HighResolutionMap.
 *  
 * This file contains all the neccesary functions to fusion and send the
 * information stored in the aux_MapCell_local structure to the 
 * HighResolutionMap.
 * 
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.03
 * \date      2010-02-04
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-09-01 \n
 *      First implementation of the LayersFusion function.
 *      
 * - Version 0.02:   Antonio MATTA                          2009-09-28 \n
 *      LayersFusionHigh method modified.
 *      
 * - Version 0.03: Thomas KOPFSTEDT                         2010-02-04 \n
 *   Changes of class names due to cleanup in "interfaces.cs", and the 
 *   other interface classed to double names for the same class
 *   
 * - Version 0.04:  Luis VALDES                             2010-04-27 \n
 *   Function ModifyLowResMap added
 *
 */
/* ######################################################################### */

using System;
using System.Collections.Generic;
using System.Text;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;

/*This is for using lists*/
using System.Collections;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_FusionTemp2Local
     *
     *  \brief  This is the class of the M44_HeightGradientEst function.
     * 
     */
    /* ------------------------------------------------------------------------- */
    static class M44_FusionTemp2Local
    {
        #region Fusion_Temporal2Local
        /* ------------------------------------------------------------------------- */
        /**	public static cl_Map2D Fusion_Temporal2Local( cl_Map2D HighResMap, 
         *                                                     aux_MapCell aux_MapCell_local)
         * 
         *  \brief The information stored in the aux_MapCell_local structure, a laser scan, is sent
         *         to the HighResMap.
         *  
         *  \param[in] cl_Map2D HighResMap
         *  \param[in] aux_MapCell aux_MapCell_local
         *
         */
        /* ------------------------------------------------------------------------- */
        public static cl_Map2D Fusion_Temporal2Local( cl_Map2D HighResMap,
                                                           aux_MapCell aux_MapCell_local,
                                                           cl_RobotGPSINSState RobotEstimate 
                                                          )
        {
            int[] RobotCell = new int[2];
            double[] RobotState = new double[2];
            HighResMap = LayersFusion(HighResMap, aux_MapCell_local);

            /*GPS and communication layers fusion*/

            RobotState[0] = RobotEstimate.RobotEstimate.td_Pos[0];
            RobotState[1] = RobotEstimate.RobotEstimate.td_Pos[1];
            RobotCell = M44_BasicClass.Coordinates2Cell_High(HighResMap, RobotState);
            if ((RobotCell[0] >= 0) && (RobotCell[0] <= (HighResMap.us_Nrows - 1)) && (RobotCell[1] >= 0) && (RobotCell[1] <= (HighResMap.us_Ncols - 1)))
            {
                HighResMap.MapCell[RobotCell[0], RobotCell[1]].c8_GPSQuality = (byte)(aux_MapCell_local.GPS_reception_quality[RobotCell[0], RobotCell[1]]);
                HighResMap.MapCell[RobotCell[0], RobotCell[1]].c8_CommQuality = (byte)(aux_MapCell_local.Communication_Quality[RobotCell[0], RobotCell[1]]);
            }

            return HighResMap;
        }
        #endregion Fusion_Temporal2Local

        #region LayersFusion
        /* ------------------------------------------------------------------------------------------------------- */
        /**	public static cl_Map2D LayersFusion(cl_Map2D Map, aux_MapCell aux_MapCell_local)
         * 
         *  \brief Algorithm to merge the processed information of the scan into 
         *         the Low Resolution Map
         *  
         * 
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] aux_MapCell aux_MapCell_local
         *  
         *  \param[out] cl_Map2D Map
         *
         */
        /* ------------------------------------------------------------------------- */
        public static cl_Map2D LayersFusion(cl_Map2D Map, aux_MapCell aux_MapCell_local )
        {
            float f_gradient_variation = (float)(M44_SensorFusionAndMappingService.i_MapRes * Math.PI / 180);  /*Absolut variation of the gradient measures*/
            float f_height_variation = 0.5f;        /*Absolut variation of the height, distance in meters*/
            byte  c8_confidence_variation = 2;      /*The number of Quads necesary to be considered a great variation on
                                                     * confidence*/
            float f_old_height;
            float f_new_height;
            float f_old_gradientX;
            float f_old_gradientY;
            float f_new_gradientX;
            float f_new_gradientY;
            int   i_confidence_inf_limit;
            int   i_confidence_sup_limit;
            byte c8_old_confidence;
            byte c8_new_confidence;
            byte c8_old_occupation;
            byte c8_new_occupation;

            /*It starts the merge*/
            for (int i = 0; i < aux_MapCell_local.us_Nrows; i++)
            {
                for (int j = 0; j < aux_MapCell_local.us_Ncols; j++)
                {
                    if (aux_MapCell_local.Confidence[i, j] > 0)
                    {
                        f_old_height    = Map.MapCell[i, j].f_Height;
                        f_old_gradientX = Map.MapCell[i, j].f_Gradient_x;
                        f_old_gradientY = Map.MapCell[i, j].f_Gradient_y;
                        c8_old_confidence = Map.MapCell[i, j].c8_Confidence;
                        c8_old_occupation = Map.MapCell[i, j].c8_Occupation;


                        f_new_height    = aux_MapCell_local.Measured_Heights[i, j];
                        f_new_gradientX = aux_MapCell_local.Measured_Gradient_X[i, j];
                        f_new_gradientY = aux_MapCell_local.Measured_Gradient_Y[i, j];
                        c8_new_confidence = aux_MapCell_local.Confidence[i, j];
                        c8_new_occupation = aux_MapCell_local.Occupation[i, j];

                        i_confidence_inf_limit = (int)(c8_old_confidence - c8_confidence_variation);
                        if (i_confidence_inf_limit < 0)
                        {
                            i_confidence_inf_limit = 0;
                        }

                        i_confidence_sup_limit = (int)(c8_old_confidence + c8_confidence_variation);
                        if (i_confidence_sup_limit > 1)
                        {
                            i_confidence_sup_limit = 1;
                        }

                        /*Now it begins comparing gradients  layers to fuse the
                          layers looking for changes into the measures.*/

                        /*Similar GradientX and  Similar GradientY*/
                        if ((similar_gradient(f_new_gradientX, f_old_gradientX, f_gradient_variation) && similar_gradient(f_new_gradientY, f_old_gradientY, f_gradient_variation)))
                        {
                            Map.MapCell[i, j].f_Gradient_x = (f_old_gradientX * c8_old_confidence + f_new_gradientX * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                            Map.MapCell[i, j].f_Gradient_y = (f_old_gradientY * c8_old_confidence + f_new_gradientY * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                            Map.MapCell[i, j].f_Height = (f_old_height * c8_old_confidence + f_new_height * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                            Map.MapCell[i, j].c8_Occupation = (byte)((c8_old_occupation * c8_old_confidence + c8_new_occupation * c8_new_confidence) / (c8_old_confidence + c8_new_confidence));
                            Map.MapCell[i, j].c8_Confidence = Math.Max(c8_old_confidence, c8_new_confidence);
                        }

                        /*Different Heights or different Gradients with new_confidence higher than old_confidence*/
                        else if (((f_old_height <= f_new_height - f_height_variation) || (f_old_height >= f_new_height + f_height_variation) ||
                                 (f_old_gradientX <= f_new_gradientX - f_gradient_variation) || (f_old_gradientX >= f_new_gradientX + f_gradient_variation) ||
                                 (f_old_gradientY <= f_new_gradientY - f_gradient_variation) || (f_old_gradientY >= f_new_gradientY + f_gradient_variation)) &&
                                 (c8_new_confidence >= i_confidence_sup_limit))
                        {
                            Map.MapCell[i, j].f_Height = f_new_height;
                            Map.MapCell[i, j].f_Gradient_x = f_new_gradientX;
                            Map.MapCell[i, j].f_Gradient_y = f_new_gradientY;
                            Map.MapCell[i, j].c8_Occupation = c8_new_occupation;
                            Map.MapCell[i, j].c8_Confidence = Math.Max(c8_old_confidence, c8_new_confidence);

                        }

                        /*Different Heights or different Gradients with new_confidence similar to old_confidence*/
                        else if (((f_old_height <= f_new_height - f_height_variation) || (f_old_height >= f_new_height + f_height_variation) ||
                                 (f_old_gradientX <= f_new_gradientX - f_gradient_variation) || (f_old_gradientX >= f_new_gradientX + f_gradient_variation) ||
                                 (f_old_gradientY <= f_new_gradientY - f_gradient_variation) || (f_old_gradientY >= f_new_gradientY + f_gradient_variation)) &&
                                 (c8_new_confidence > i_confidence_inf_limit) && (c8_new_confidence < i_confidence_sup_limit))
                        {
                            Map.MapCell[i, j].f_Height = f_new_height;
                            Map.MapCell[i, j].f_Gradient_x = f_new_gradientX;
                            Map.MapCell[i, j].f_Gradient_y = f_new_gradientY;
                            Map.MapCell[i, j].c8_Occupation = c8_new_occupation;
                            Map.MapCell[i, j].c8_Confidence = (byte)(1);
                        }
                    }
                }
            }
            
            return Map;
        }
        #endregion LayersFusion

        #region ModifyLowResMap
        /* ------------------------------------------------------------------------------------------------------- */
        /**	public static cl_Map2D ModifyLowResMap(cl_Map2D Map, cl_Map2D High2LowRes_Map)
         * 
         *  \brief Algorithm to merge the processed information of the scan into 
         *         the Low Resolution Map
         *  
         * 
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] cl_Map2D High2LowRes_Map
         *  \param[in] ArrayList CellList
         *  
         *  \param[out] cl_Map2D Map
         *
         */
        /* ------------------------------------------------------------------------- */
        public static cl_Map2D ModifyLowResMap(cl_Map2D LowResMap, cl_Map2D High2LowRes_Map, ArrayList CellList)
        {
            /*Absolut variation of the gradient measures*/
            float f_gradient_variation = (float)(M44_SensorFusionAndMappingService.i_MapRes * Math.PI / 180);
            /*Absolut variation of the height, distance in meters*/
            float f_height_variation = 0.5f;
            /* The number of Quads necesary to be considered
             * a great variation on confidence*/
            byte c8_confidence_variation = 2;

            //Old values in LowResMap
            float f_old_height;
            float f_old_gradientX;
            float f_old_gradientY;
            byte c8_old_confidence;
            byte c8_old_occupation;
            //New values measured by the sensors
            float f_new_height;
            float f_new_gradientX;
            float f_new_gradientY;
            byte c8_new_confidence;
            byte c8_new_occupation;
            //Confidence Limits
            int i_confidence_inf_limit;
            int i_confidence_sup_limit;
            //LowResMap cell where the High2LowRes_Map is centered
            int[] CenterCell = new int[2];
            //Coordinates of the center of High2LowRes_Map
            double[] CenterCoordinates = new double[2];

            CenterCoordinates[0] = High2LowRes_Map.Center.d_x;
            CenterCoordinates[1] = High2LowRes_Map.Center.d_y;
            CenterCell = M44_BasicClass.Coordinates2Cell(LowResMap, CenterCoordinates);


            /*It starts the merge*/
            for (int i = 0; i < High2LowRes_Map.us_Nrows; i++)
            {
                for (int j = 0; j < High2LowRes_Map.us_Ncols; j++)
                {
                    //Equivalence between the cells in High2LowRes_Map and LowResMap
                    int I = CenterCell[0] - (High2LowRes_Map.us_Nrows / 2 - 1) + i;
                    int J = CenterCell[1] - (High2LowRes_Map.us_Ncols / 2) + j;

                    if (High2LowRes_Map.MapCell[i, j].c8_Confidence > 0)
                    {
                        f_old_height = LowResMap.MapCell[I, J].f_Height;
                        f_old_gradientX = LowResMap.MapCell[I, J].f_Gradient_x;
                        f_old_gradientY = LowResMap.MapCell[I, J].f_Gradient_y;
                        c8_old_confidence = LowResMap.MapCell[I, J].c8_Confidence;
                        c8_old_occupation = LowResMap.MapCell[I, J].c8_Occupation;

                        f_new_height = High2LowRes_Map.MapCell[i, j].f_Height;
                        f_new_gradientX = High2LowRes_Map.MapCell[i, j].f_Gradient_x;
                        f_new_gradientY = High2LowRes_Map.MapCell[i, j].f_Gradient_y;
                        c8_new_confidence = High2LowRes_Map.MapCell[i, j].c8_Confidence;
                        c8_new_occupation = High2LowRes_Map.MapCell[i, j].c8_Occupation;

                        i_confidence_inf_limit = (int)(c8_old_confidence - c8_confidence_variation);
                        if (i_confidence_inf_limit < 0)
                        {
                            i_confidence_inf_limit = 0;
                        }

                        i_confidence_sup_limit = (int)(c8_old_confidence + c8_confidence_variation);
                        if (i_confidence_sup_limit > 1)
                        {
                            i_confidence_sup_limit = 1;
                        }

                        /*Now it begins comparing gradients layers to fuse the
                          layers looking for changes into the measures.*/

                        /*Similar GradientX and  Similar GradientY*/
                        if ((similar_gradient(f_new_gradientX, f_old_gradientX, f_gradient_variation) && similar_gradient(f_new_gradientY, f_old_gradientY, f_gradient_variation)))
                        {
                            LowResMap.MapCell[I, J].f_Gradient_x = (f_old_gradientX * c8_old_confidence + f_new_gradientX * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                            LowResMap.MapCell[I, J].f_Gradient_y = (f_old_gradientY * c8_old_confidence + f_new_gradientY * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                            LowResMap.MapCell[I, J].f_Height = (f_old_height * c8_old_confidence + f_new_height * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                            LowResMap.MapCell[I, J].c8_Occupation = (byte)((c8_old_occupation * c8_old_confidence + c8_new_occupation * c8_new_confidence) / (c8_old_confidence + c8_new_confidence));
                            LowResMap.MapCell[I, J].c8_Confidence = Math.Max(c8_old_confidence, c8_new_confidence);
                        }

                        /*Different Heights or different Gradients with new_confidence higher than old_confidence*/
                        else if (((f_old_height <= f_new_height - f_height_variation) || (f_old_height >= f_new_height + f_height_variation) ||
                                (f_old_gradientX <= f_new_gradientX - f_gradient_variation) || (f_old_gradientX >= f_new_gradientX + f_gradient_variation) ||
                                (f_old_gradientY <= f_new_gradientY - f_gradient_variation) || (f_old_gradientY >= f_new_gradientY + f_gradient_variation)) &&
                                (c8_new_confidence >= i_confidence_sup_limit))
                        {
                            LowResMap.MapCell[I, J].f_Height = f_new_height;
                            LowResMap.MapCell[I, J].f_Gradient_x = f_new_gradientX;
                            LowResMap.MapCell[I, J].f_Gradient_y = f_new_gradientY;
                            LowResMap.MapCell[I, J].c8_Occupation = c8_new_occupation;
                            LowResMap.MapCell[I, J].c8_Confidence = Math.Max(c8_old_confidence, c8_new_confidence);
                        }

                        /*Different Heights or Different Gradients with new_confidence similar to old_confidence*/
                        else if (((f_old_height <= f_new_height - f_height_variation) || (f_old_height >= f_new_height + f_height_variation) ||
                                 (f_old_gradientX <= f_new_gradientX - f_gradient_variation) || (f_old_gradientX >= f_new_gradientX + f_gradient_variation) ||
                                 (f_old_gradientY <= f_new_gradientY - f_gradient_variation) || (f_old_gradientY >= f_new_gradientY + f_gradient_variation)) &&
                                 (c8_new_confidence > i_confidence_inf_limit) && (c8_new_confidence < i_confidence_sup_limit))
                        {
                            LowResMap.MapCell[I, J].f_Height = f_new_height;
                            LowResMap.MapCell[I, J].f_Gradient_x = f_new_gradientX;
                            LowResMap.MapCell[I, J].f_Gradient_y = f_new_gradientY;
                            LowResMap.MapCell[I, J].c8_Occupation = c8_new_occupation;
                            LowResMap.MapCell[I, J].c8_Confidence = (byte)(1);
                        }

                        /*Create Cell and add it to the cell list*/
                        cl_UpdatedCell Cell = new cl_UpdatedCell((ushort)i, (ushort)j);
                        Cell.MapCell = LowResMap.MapCell[i, j];
                        CellList.Add(Cell);
                    }
                    /*The layers wich no need a special fusion are copied*/
                    if ((LowResMap.MapCell[I, J].c8_GPSQuality != High2LowRes_Map.MapCell[i, j].c8_GPSQuality) && (High2LowRes_Map.MapCell[i, j].c8_Confidence > 0))
                    {
                        LowResMap.MapCell[I, J].c8_GPSQuality = (byte)High2LowRes_Map.MapCell[i, j].c8_GPSQuality;

                    }
                    else
                    {
                        //Nothing to do
                    }

                    if ((LowResMap.MapCell[I, J].c8_CommQuality != High2LowRes_Map.MapCell[i, j].c8_CommQuality) && (High2LowRes_Map.MapCell[i, j].c8_Confidence > 0))
                    {
                        LowResMap.MapCell[I, J].c8_CommQuality = (byte)High2LowRes_Map.MapCell[i, j].c8_CommQuality;

                    }
                    else
                    {
                        //Nothing to do
                    }
                }
            }

            return LowResMap;
        }
        #endregion ModifyLowResMap

        #region similar_gradient
        /* ------------------------------------------------------------------------- */
        /**	public static boolsimilar_gradient( float f_new_gradientX, 
         *                                        float f_old_gradientX, 
         *                                        float f_gradient_variation )
         * 
         *  \brief 
         *  
         *  \param[in] f_new_gradientX
         *  \param[in] f_old_gradientX
         *  \param[in] f_gradient_variation
         *  
         */
        /* ------------------------------------------------------------------------- */
        public static bool similar_gradient( float f_gradient_a,
                                              float f_gradient_b,
                                              float f_gradient_variation)
        {
            float f_angle_a;
            float f_angle_b;

            f_angle_a = (float)Math.Atan(f_gradient_a);
            f_angle_b = (float)Math.Atan(f_gradient_b);

            if ((f_angle_b < (f_angle_a + f_gradient_variation)) && (f_angle_b > (f_angle_a - f_gradient_variation)))
            {
                return true;
            }
            else
            {
                return false;
            }
        }
        #endregion similar_gradient
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */
